//
// Created by LinorInk on 2024/3/17.
//

#include "servo.h"
#include "task.h"

static Servo_HandleTypedef servo[SERVO_NUM];
static uint8_t servo_num = 0;

static void servo_disPlay_task(void);

void servo_regist(Servo_InitTypedef *servo_info) {
    servo[servo_num].htim = servo_info->htim;
    servo[servo_num].TIM_Channel = servo_info->TIM_Channel;
    servo[servo_num].id = servo_num;
    servo_num++;

    __HAL_TIM_SET_PRESCALER(servo_info->htim, 1680 - 1);
    __HAL_TIM_SET_AUTORELOAD(servo_info->htim, 1000 - 1);
    HAL_TIM_PWM_Start(servo_info->htim, servo_info->TIM_Channel);
}

void servo_init() {
    Servo_InitTypedef servo_info = {&htim8, TIM_CHANNEL_3};
    servo_regist(&servo_info);

    servo_info.TIM_Channel = TIM_CHANNEL_4;
    servo_regist(&servo_info);

    servo_info.htim = &htim1;
    servo_info.TIM_Channel = TIM_CHANNEL_3;
    servo_regist(&servo_info);

    servo_info.htim = &htim4;
    servo_info.TIM_Channel = TIM_CHANNEL_1;
    servo_regist(&servo_info);

    servo_info.htim = &htim4;
    servo_info.TIM_Channel = TIM_CHANNEL_2;
    servo_regist(&servo_info);

    servo_info.htim = &htim4;
    servo_info.TIM_Channel = TIM_CHANNEL_3;
    servo_regist(&servo_info);

    servo_info.htim = &htim3;
    servo_info.TIM_Channel = TIM_CHANNEL_3;
    servo_regist(&servo_info);

    servo_info.htim = &htim8;
    servo_info.TIM_Channel = TIM_CHANNEL_1;
    servo_regist(&servo_info);

    servo_info.htim = &htim1;
    servo_info.TIM_Channel = TIM_CHANNEL_1;
    servo_regist(&servo_info);

    servo_info.htim = &htim5;
    servo_info.TIM_Channel = TIM_CHANNEL_3;
    servo_regist(&servo_info);

    servo_info.TIM_Channel = TIM_CHANNEL_2;
    servo_regist(&servo_info);

    servo_info.TIM_Channel = TIM_CHANNEL_1;
    servo_regist(&servo_info);

    servo_info.htim = &htim11;
    servo_info.TIM_Channel = TIM_CHANNEL_1;
    servo_regist(&servo_info);

    servo_info.htim = &htim9;
    servo_info.TIM_Channel = TIM_CHANNEL_2;
    servo_regist(&servo_info);

    servo_info.htim = &htim9;
    servo_info.TIM_Channel = TIM_CHANNEL_1;
    servo_regist(&servo_info);

    servo_info.htim = &htim2;
    servo_info.TIM_Channel = TIM_CHANNEL_2;
    servo_regist(&servo_info);

    servo_info.htim = &htim2;
    servo_info.TIM_Channel = TIM_CHANNEL_4;
    servo_regist(&servo_info);

    servo_info.htim = &htim3;
    servo_info.TIM_Channel = TIM_CHANNEL_2;
    servo_regist(&servo_info);

    servo_info.htim = &htim1;
    servo_info.TIM_Channel = TIM_CHANNEL_2;
    servo_regist(&servo_info);
}

/**
 * 设置舵机角度
 * @param servoId 电机序号
 * @param angle   需要设置的角度 范围是0 ~ 180
 */
void servo_setAngle(uint8_t servoId, uint16_t angle) {
    servo[servoId].angle = angle;
    //if (servoId <18)
        __HAL_TIM_SetCompare(servo[servoId].htim, servo[servoId].TIM_Channel, (uint32_t) (angle * 100 / 270 + 25));
    //else if (servoId == 18) {
        //__HAL_TIM_SetCompare(servo[servoId].htim, servo[servoId].TIM_Channel, (uint32_t) (angle * 100 / 180 + 25));
    //}
}

void servo_setAction(const uint16_t *angles) {
    for(int i = 0; i < servo_num-1; i++) {
        servo[i].angle = angles[i];
        __HAL_TIM_SetCompare(servo[i].htim, servo[i].TIM_Channel, (uint32_t) (angles[i] * 100 / 270 + 25));
    }
}

static uint8_t *dp_id;
static uint8_t *dp_angles;
static uint16_t *dp_dt;
static uint8_t dp_length;


static taskTypedef Servo_DisplayTask;
static uint8_t isDisplay = 0;

/**
 * 电机播放预设旋转
 * @param angles 角度数组
 * @param dt 间隔时长数组
 * @param length 长度数组
 */
void servo_display(uint8_t *id, uint8_t *angles, uint16_t *dt, uint8_t length) {
    dp_id = id;
    dp_angles = angles;
    dp_dt = dt;
    dp_length = length;

    isDisplay = 1;
    Task_regist(&Servo_DisplayTask, 0, 0xff, servo_disPlay_task);
}

static uint8_t dp_count = 0;

static void servo_disPlay_task(void) {
    if(dp_count < dp_length) {
        servo_setAngle(dp_id[dp_count], dp_angles[dp_count]);
        Servo_DisplayTask.tick = dp_dt[dp_count];
        dp_count++;
    } else {
        dp_count = 0;
    }
}